"""
--------------------------------------------------------------------------------
功能:PS2手柄控制机械臂程序
日期:2025-06-06
作者:Unirobot
QQ交流群:811348489
寄语：代码中能遇见你，真好，有你，真好.
署名：有你同创智能机器人科技(北京)有限公司
--------------------------------------------------------------------------------
"""

import time
import queue
import inputs
import threading
from src.joy import joy_module
from src.arm_control import custom_font

# =======================================加载自定义字体
time.sleep(1)
font = custom_font.CustomFont()
print(font.color_text(font.compony_msg, color="cyan", bold=True))
print(font.color_text(font.font_load_success, color="green", bold=True))

# =======================================创建线程
arm_command_queue = queue.Queue(maxsize=1)
joystick_action_move = False

# =======================================监听器实例
gamepad = joy_module.GamepadListener(device_path='/dev/input/js0')
gamepad.start()
print(font.color_text(font.ps2_load_success, color="green", bold=True))


# 按钮高亮函数：按下=绿色，否则灰色
def format_button(name, pressed):
    if pressed:
        return font.color_text(f"{name}:1", color="green", bold=True)
    else:
        return font.color_text(f"{name}:0", color="bright_magenta")


# 维护四个摇杆状态
axis_states = {
    'Left Stick X': 0.0,
    'Left Stick Y': 0.0,
    'Right Stick X': 0.0,
    'Right Stick Y': 0.0
}

# 按键状态
button_states = {
    'Left Shoulder': False,'L2': False,'Up': False,'Down': False,'Left': False,'Right': False,
    'Right Shoulder': False,'R2': False,'Y': False,'A': False,'X': False,'B': False,
    'Select': False,'Start': False,'Mode/Xbox': False,
    'Left Thumb': False,       # 左摇杆按下
    'Right Thumb': False       # 右摇杆按下
}

try:
    print(font.color_text(font.ps2_module_commond, color="cyan", bold=True))
    while True:
        # 更新状态
        events = gamepad.return_status(clear_queue=True)
        for event in events:
            if event['type'] == 'button':
                button_states[event['name']] = bool(event['value'])

            elif event['type'] == 'axis':
                if event['name'] == 'D-pad X':
                    button_states['Left'] = (event['value'] == -32767)
                    button_states['Right'] = (event['value'] == 32767)
                elif event['name'] == 'D-pad Y':
                    button_states['Up'] = (event['value'] == -32767)
                    button_states['Down'] = (event['value'] == 32767)
                elif event['name'] == 'LT':
                    button_states['L2'] = (event['value'] == 32767)
                elif event['name'] == 'RT':
                    button_states['R2'] = (event['value'] == 32767)
                elif event['name'] in axis_states:
                    axis_states[event['name']] = event['value']

        #获取四个摇杆值并对摇杆值也加颜色（示例：左右摇杆分别用黄色和青色）
        left_x  = font.color_text(f"LX:{axis_states['Left Stick X']:6.0f}", color="yellow")
        left_y  = font.color_text(f"LY:{axis_states['Left Stick Y']:6.0f}", color="yellow")
        right_x = font.color_text(f"RX:{axis_states['Right Stick X']:6.0f}", color="cyan")
        right_y = font.color_text(f"RY:{axis_states['Right Stick Y']:6.0f}", color="cyan")

        #把按钮状态转成 0/1 并组合成一行字符串，构建带颜色的按钮字符串
        buttons_str = " ".join([format_button(name, pressed) for name, pressed in button_states.items()])

        # 打印所有按钮状态和摇杆值
        print(f"{buttons_str} | {left_x} {left_y} {right_x} {right_y}")

        time.sleep(0.05)  # 稍微降低刷新频率，防止输出过快

except KeyboardInterrupt:
    print("\n程序退出")
finally:
    gamepad.stop()